#include "lcd.h"
#include "pins_def.h"

#define READY PA_7
#define RD_CLK PA_6
#define RD_EN PA_5
#define FIN_X_SEL PA_2
#define DOUT_0 PF_4
#define CLR PF_3
#define DOUT_X PA_4
#define DOUT_Y PB_7
#define MD_1 PB_2
#define MD_0 PB_3

static const double std_frequence = 50000000.0;
static const int freq_divider = 64;

int tmp;
String out_str;

int set_mode = 1;
int current_mode = 0;
boolean use_dvd = false;
boolean reverse = true;

unsigned int val0 = 0;
unsigned int valx = 0;
unsigned int valy = 0;

double frequence_x = 0; 
double period_x = 0;
double duty_ratio = 0; 
double delta_time = 0;
double delta_time_real = 0;
double delta_time_offset = 0;

char str_buf[1024];
char lcd_str_fmt[50];
char lcd_str[17];
char lcd_str_ld[17];
char console_str[1024];
char unit_lvl;
double lcd_value;
int digits = 0;

boolean _in_interrupt = false;

double mabs(double x) {
    if (x >= 0) return x;
    else return 0 - x;
}

void printMode() {
    char *mode_str;
    char *mode_sel_str;
    switch (set_mode) {
        case 1: 
            mode_str = new char[33];
//            sprintf(mode_str, "Freq%s", (use_dvd ? ">10MHz" : "<10MHz"));
            sprintf(mode_str, "\x80\x81(%s)", (use_dvd ? "GearB" : "GearA"));
            mode_sel_str = "1>\x82\x83\x84  2>\x85\x86";
            break;
        case 2: 
            mode_str = new char[33];
            sprintf(mode_str, "\x82\x83\x84%c    ", (reverse ? '.' : ' '));
            mode_sel_str = "1>\x80\x81  2>\x85\x86  ";
            break;
        case 3: 
            if (delta_time_offset < 0.000000001) {
                unit_lvl = ' ';
                lcd_value = 0.0;
            } else if (delta_time_offset < 0.000001) {
                unit_lvl = 'n';
                lcd_value = delta_time_offset * 1000000000.0;
            } else if (delta_time < 0.001) {
                unit_lvl = 'u';
                lcd_value = delta_time_offset * 1000000.0;
            } else if (delta_time < 1.0) {
                unit_lvl = 'm';
                lcd_value = delta_time_offset * 1000.0;
            } else {
                unit_lvl = ' ';
                lcd_value = delta_time_offset;
            }
            sprintf(console_str, "dtofs: %f;    vl: %f\n", delta_time_offset, lcd_value);
            Serial.write(console_str);
            mode_str = "\x85\x86       ";
            if (lcd_value != 0.0)  {
                Serial.write("should see!!!\n");
                mode_str = new char[33];
                sprintf(mode_str, "\x85\x86(%c%3d%cs)", (lcd_value < 0.0) ? '+' : '-', (int)(lcd_value >= 0 ? lcd_value : 0 - lcd_value), unit_lvl);
            }
            mode_sel_str = "1>\x80\x81  2>\x82\x83\x84";
            break;
    }
    sprintf(lcd_str, "\x89\x8a:%s", mode_str);
    LCD_Enable(true);
    LCD_print(mode_sel_str, 0, 0);
    LCD_print(lcd_str, 1, 0);
    LCD_Enable(false);
}

// Read data using interrupts on rd_clk
int _read_data_iter = 0;
boolean _read_data_ok = false;
unsigned int _chk0, _chkx, _chky;
void readdata_start();
void readdata_int();
void readdata_end();
void readdata_start() {
    _read_data_iter = 0;
    _read_data_ok = false;
    val0 = 0; valx = 0; valy = 0;
    _chk0 = 0; _chkx = 0; _chky = 0;
    attachInterrupt(RD_CLK, readdata_int, RISING);
    delayMicroseconds(10);
    digitalWrite(RD_EN, HIGH);
    delayMicroseconds(10);
}
void readdata_int() {
//  sprintf(console_str, "int% 2d", _read_data_iter);
//  Serial.write(console_str);
    int digit_0, digit_x, digit_y;
    delayMicroseconds(10);
    digit_0 = digitalRead(DOUT_0);
//  sprintf(console_str, "    dout0=%d\n", digit_0);
//  Serial.write(console_str);
    digit_x = digitalRead(DOUT_X);
    digit_y = digitalRead(DOUT_Y);
    if (_read_data_iter > 31) {
        _chk0 = (_chk0 << 1) + digit_0;
        _chkx = (_chkx << 1) + digit_x;
        _chky = (_chky << 1) + digit_y;
    } else {
        val0 = (val0 << 1) + digit_0;
        valx = (valx << 1) + digit_x;
        valy = (valy << 1) + digit_y;
    }
    if (++_read_data_iter > 35) {
        readdata_end();
    }
}
void readdata_end() {
  Serial.write("end\n");
    _read_data_ok = true;
    digitalWrite(RD_EN, LOW);
    detachInterrupt(RD_CLK);
}
boolean check_data(uint32_t val, uint8_t chk) {
    uint8_t _chk = 0;
    uint32_t _mask = 1;
    for (int i = 31; i >= 0; i--) {
        _chk = _chk ^ (((val >> i) & _mask) << (i % 4));
    }
    return chk == _chk;
}

// User button interrupts
void changeMode0() {
    if (_in_interrupt) return;
    _in_interrupt = true;
    Serial.write("btn click\n");
    switch (set_mode) {
        case 1: set_mode = 2; break;
        case 2: set_mode = 3; break;
        case 3: set_mode = 1; break;
        default: set_mode = 1; break;
    }
    printMode();
    _in_interrupt = false;
}
void changeMode1() {
    Serial.write("btn click\n");
    switch (set_mode) {
        case 1: set_mode = 2; break;
        case 2: set_mode = 1; break;
        case 3: set_mode = 1; break;
        default: set_mode = 1; break;
    }
    printMode();
}
void changeMode2() {
    Serial.write("btn click\n");
    switch (set_mode) {
        case 1: set_mode = 3; break;
        case 2: set_mode = 3; break;
        case 3: set_mode = 2; break;
        default: set_mode = 1; break;
    }
    printMode();
}
void toggleDvd() {
  Serial.write("btn click\n");
  if (set_mode == 1) {
      use_dvd = !use_dvd;
      digitalWrite(FIN_X_SEL, use_dvd);
  } else if (set_mode == 2) {
      reverse = !reverse;
  } else if (set_mode == 3) {
      delta_time_offset = delta_time_real;
  }
  printMode();
}

void modeFreq() {
    sprintf(str_buf, "Data 0: %08x %x, Data X: %08x %x, Data Y: %08x %x\n   Frequence: %f\n", val0, _chk0, valx, _chkx, valy, _chky, frequence_x);
    Serial.write(str_buf);
    if (frequence_x > 1000000.0) {
        unit_lvl = 'M';
        lcd_value = frequence_x / 1000000.0;
    } else if (frequence_x > 1000.0) {
        unit_lvl = 'k';
        lcd_value = frequence_x / 1000.0;
    } else {
        unit_lvl = ' ';
        lcd_value = frequence_x;
    }
    if (lcd_value < 1) digits = 0;
    else if (lcd_value < 10) digits = 1;
    else if (lcd_value < 100) digits = 2;
    else if (lcd_value < 1000) digits = 3;
    else digits = 0;
    sprintf(lcd_str_fmt, "\x80\x81:%%%d.%df %cHz              ", (digits ? digits : 1), 6 - digits, unit_lvl);
    sprintf(lcd_str_ld, lcd_str_fmt, lcd_value);
    if (period_x < 0.000000001) {
        unit_lvl = ' ';
        lcd_value = 0.0;
    } else if (period_x < 0.000001) {
        unit_lvl = 'n';
        lcd_value = period_x * 1000000000.0;
    } else if (period_x < 0.001) {
        unit_lvl = 'u';
        lcd_value = period_x * 1000000.0;
    } else if (period_x < 1.0) {
        unit_lvl = 'm';
        lcd_value = period_x * 1000.0;
    } else {
        unit_lvl = ' ';
        lcd_value = period_x;
    }
    lcd_value = (double)((int)(lcd_value * 1000)) / 1000;
    if (lcd_value < 1) digits = 1;
    else if (lcd_value < 10) digits = 1;
    else if (lcd_value < 100) digits = 2;
    else if (lcd_value < 1000) digits = 3;
    else if (lcd_value < 10000) digits = 4;
    else digits = 1;
    lcd_value = (double)((int)(lcd_value * pow(10, 6 - digits) + 0.5)) / pow(10, 6 - digits);
    if (lcd_value < 1) digits = 1;
    else if (lcd_value < 10) digits = 1;
    else if (lcd_value < 100) digits = 2;
    else if (lcd_value < 1000) digits = 3;
    else if (lcd_value < 10000) digits = 4;
    else digits = 1;
    sprintf(lcd_str_fmt, "\x95\x96:%%%d.%df%cs", (digits ? digits : 1), 5 - digits, unit_lvl);
    sprintf(lcd_str, lcd_str_fmt, lcd_value);
}
void modeDuty() {
    duty_ratio = ((double)valy) / ((double)val0);
    if (reverse)  duty_ratio = 1 - duty_ratio;
    sprintf(console_str, "Data 0: %08x, Data X: %08x, Data Y: %08x\n   Duty ratio: %06.4f %%\n", val0, valx, valy, duty_ratio * 100);
    Serial.write(console_str);
    sprintf(lcd_str_ld, "\x82\x83\x84:         ");
    sprintf(lcd_str, "%2.2f %%                      ", duty_ratio * 100);
}
void modeDeltaTime() {
    delta_time_real = ((double)valy) / ((double)val0) / frequence_x;
    delta_time = delta_time_real - delta_time_offset;
    if (delta_time < 0) delta_time += period_x;
//    if (delta_time > period_x / 2) delta_time = delta_time - period_x;
    sprintf(console_str, "Data 0: %08x %x, Data X: %08x %x, Data Y: %08x\n   Delta time: %06f mm\n", val0, _chk0, valx, _chkx, valy, delta_time * 1000);
    Serial.write(console_str);
    if (mabs(delta_time) < 0.000000001) {
        unit_lvl = ' ';
        lcd_value = 0.0;
    } else if (mabs(delta_time) < 0.000001) {
        unit_lvl = 'n';
        lcd_value = delta_time * 1000000000.0;
    } else if (mabs(delta_time) < 0.001) {
        unit_lvl = 'u';
        lcd_value = delta_time * 1000000.0;
    } else if (mabs(delta_time) < 1.0) {
        unit_lvl = 'm';
        lcd_value = delta_time * 1000.0;
    } else {
        unit_lvl = ' ';
        lcd_value = delta_time;
    }
    lcd_value = (double)((int)(lcd_value * 1000)) / 1000;
    sprintf(console_str, "%f\n", lcd_value);
    Serial.write(console_str);
    if (lcd_value < 1) digits = 1;
    else if (lcd_value < 10) digits = 1;
    else if (lcd_value < 100) digits = 2;
    else if (lcd_value < 1000) digits = 3;
    else if (lcd_value < 10000) digits = 4;
    else digits = 1;
    lcd_value = (double)((int)(lcd_value * pow(10, 5 - digits) + 0.5)) / pow(10, 5 - digits);
    if (lcd_value < 1) digits = 1;
    else if (lcd_value < 10) digits = 1;
    else if (lcd_value < 100) digits = 2;
    else if (lcd_value < 1000) digits = 3;
    else if (lcd_value < 10000) digits = 4;
    else digits = 1;
    if (lcd_value < 0) {
        sprintf(lcd_str_ld, "Y \x8b\x97 X:       ");
    } else {
        sprintf(lcd_str_ld, "Y \x87\x88 X:       ");
    }
    sprintf(console_str, "lcd_value: %d %f\n", digits, lcd_value);
    Serial.write(console_str);
    sprintf(lcd_str_fmt, "%%%d.%df %cs              ", (digits ? digits : 1), 5 - digits, unit_lvl);
    sprintf(lcd_str, lcd_str_fmt, lcd_value >= 0 ? lcd_value : 0 - lcd_value);
}

// Timeout
boolean timeout;
unsigned long timeoutStartMillis = 0;
unsigned long timeoutNowMillis = 0;

void init() {
    digitalWrite(CLR, LOW);
    detachInterrupt(RD_CLK);
    digitalWrite(RD_EN, LOW);
    digitalWrite(MD_1, LOW); digitalWrite(MD_0, LOW);
    digitalWrite(FIN_X_SEL, LOW);
    current_mode = set_mode;
    printMode();
}

void setup()
{
    // put your setup code here, to run once:
    unuse();
    Serial.begin(9600);

    pinMode(READY, INPUT);
    pinMode(RD_CLK, INPUT);
    pinMode(RD_EN, OUTPUT);
    pinMode(FIN_X_SEL, OUTPUT);
    pinMode(DOUT_0, INPUT);
    pinMode(CLR, OUTPUT);
    pinMode(DOUT_X, INPUT);
    pinMode(DOUT_Y, INPUT);
    pinMode(MD_1, OUTPUT);
    pinMode(MD_0, OUTPUT);

    LCD_Initial();
    delay(1);
    LCD_Enable(true);
    LCD_Clear();
    LCD_print("2015\x92\x93", 0, 1);
    delay(2000);
    LCD_print("\x80\x81\x94    ", 0, 1);
    delay(2000);
    LCD_Enable(false);
    
//    attachInterrupt(LCD_S1, changeMode1, RISING);
//    attachInterrupt(LCD_S2, changeMode2, RISING);
    attachInterrupt(LCD_S1, changeMode0, RISING);
    attachInterrupt(LCD_S3, toggleDvd, RISING);
    interrupts();
}

void loop()
{
    sprintf(console_str, "set mode : %d\n", set_mode);
    Serial.write(console_str);
    if (set_mode < 1 || set_mode > 3) return;
    if (set_mode != current_mode) init();
    // Prepare to count
    digitalWrite(RD_EN, LOW);
    digitalWrite(CLR, 1);
    delayMicroseconds(10);
    digitalWrite(CLR, 0);

    // Count for 1s
    sprintf(console_str, "md=%d%d\n", (current_mode & 2) ? 1 : 0, (current_mode & 1) ? 1 : 0);
    Serial.write(console_str);
    digitalWrite(MD_1, (current_mode & 2) ? HIGH : LOW);
    digitalWrite(MD_0, (current_mode & 1) ? HIGH : LOW);
    delay(1000);
    digitalWrite(MD_1, LOW); digitalWrite(MD_0, LOW);

    // Wait for ready
    timeoutStartMillis = millis();
//    readdata_start();
    while (!digitalRead(READY) && (millis() - timeoutStartMillis) < 1000) delay(1);
    if (!digitalRead(READY)) {
        Serial.write("Warning: Ready timeout!\n");
        LCD_Enable(true);
        LCD_print("               ", 2, 0);
        LCD_print("    \x89\x8a\x8b\x85    ", 3, 0);
        LCD_Enable(false);
        return;
    }
    Serial.write("Warning: Ready!\n");
    delayMicroseconds(20);
    // Wait for data
    timeoutStartMillis = millis();
    readdata_start();
    while (!_read_data_ok && (millis() - timeoutStartMillis) < 1000) delay(1);
    if (!_read_data_ok) {
        Serial.write("Warning: Read data timeout!\n");
        LCD_Enable(true);
        LCD_print("               ", 2, 0);
        LCD_print("    \x8c\x8d\x8b\x85    ", 3, 0);
        LCD_Enable(false);
        return;
    }
    // Check data received
    if (!(check_data(val0, _chk0) && check_data(valx, _chkx) && check_data(valy, _chky))) {
        Serial.write("Warning: Data transfer error!\n");
        LCD_Enable(true);
        LCD_print("               ", 2, 0);
        LCD_print("\x8c\x8d\x8e\x8f  \x90\x91..", 3, 0);
        LCD_Enable(false);
        return;
    }

    // Do calculate and print the result
    frequence_x = ((double)valx) / ((double)val0) * std_frequence * (use_dvd ? freq_divider : 1);
    period_x = 1.0 / frequence_x;
    if (current_mode == 1) {
        modeFreq();
    } else if (current_mode == 2) {
        modeDuty();
    } else if (current_mode == 3) {
        modeDeltaTime();
    }
    LCD_Enable(true);
    LCD_print(lcd_str_ld, 2, 0);
    LCD_print(lcd_str, 3, 0);
    LCD_Enable(false);
}
